/*
 * Copyright (c) 2006-2018, RT-Thread Development Team
 *rt_interrupt_from_thread
 * SPDX-License-Identifier: GPL-2.0 License
 *
 * Change Logs:
 * Date           Author       Notes
 * 2021/02/01     BalanceTWK   The unify TriCore porting code.
 */

#include <rthw.h>
#include <rtthread.h>
#include "cpuport.h"

/* External RT-Thread variables */
extern struct rt_thread *rt_current_thread;
extern rt_uint32_t rt_thread_ready_priority_group;
extern rt_list_t rt_thread_priority_table[];
extern volatile rt_uint8_t rt_interrupt_nest;

/* RT-Thread context switch variables */
volatile rt_ubase_t rt_interrupt_from_thread = 0;
volatile rt_ubase_t rt_interrupt_to_thread = 0;
volatile rt_uint32_t rt_thread_switch_interrupt_flag = 0;
#include "IfxCpu_Trap.h"
#include <Cpu/Std/Ifx_Types.h>
#include "Cpu/Std/IfxCpu_Intrinsics.h"
#include "Tricore/Compilers/Compilers.h"
#include "Cpu/Irq/IfxCpu_Irq.h"
#include "IfxInt_reg.h"
#include "IfxCpu_reg.h"

/* Define missing TRICORE_TRAP_SYSCALL */
#ifndef TRICORE_TRAP_SYSCALL
#define TRICORE_TRAP_SYSCALL    6U  /* System call trap class */
#endif

static IfxStm_Timer tricore_timers[2];
static volatile Ifx_STM *const STMs[2] = {&MODULE_STM0, &MODULE_STM1};
static volatile Ifx_SRC_SRCR *const GPSR[2] = {&SRC_GPSR_GPSR0_SR0, &SRC_GPSR_GPSR1_SR0};



typedef struct UpperCtxInfo{
    unsigned long  _PCXI;
    unsigned long  _PSW;
    unsigned long*  _A10;
    unsigned long*  _A11;
    unsigned long  _D8;
    unsigned long  _D9;
    unsigned long  _D10;
    unsigned long  _D11;
    unsigned long*  _A12;
    unsigned long*  _A13;
    unsigned long*  _A14;
    unsigned long*  _A15;
    unsigned long  _D12;
    unsigned long  _D13;
    unsigned long  _D14;
    unsigned long  _D15;
}UpperCtx_T,*UpperCtx_Ptr;

rt_base_t rt_hw_interrupt_disable(void)
{
    rt_base_t level;
    level = IfxCpu_disableInterrupts();
    return level;
}

void rt_hw_interrupt_enable(rt_base_t level)
{
    restoreInterrupts((boolean)level);
}

void rt_hw_systick_init( void )
{
  IfxStm_Timer_Config timer_config;
  IfxStm_Timer_initConfig(&timer_config, STMs[TRICORE_CPU_ID]);

  timer_config.base.frequency = RT_TICK_PER_SECOND;
  timer_config.base.isrPriority = 2;
  IfxStm_Timer_init(&tricore_timers[TRICORE_CPU_ID], &timer_config);
  IfxStm_Timer_run(&tricore_timers[TRICORE_CPU_ID]);
}

/* inline */ void trigger_scheduling(void)
{
    /* 直接进行上下文切换，避免无限递归 */
    rt_kprintf("[DEBUG] trigger_scheduling called\n");
    
    /* 检查是否有线程需要调度 */
    if (rt_current_thread != RT_NULL)
    {
        rt_kprintf("[DEBUG] Current thread: %s\n", rt_current_thread->name);
        
        /* 触发调度器进行线程切换 */
        rt_schedule();
    }
    else
    {
        rt_kprintf("[DEBUG] No current thread, scheduler not ready\n");
    }
}

/**
 * This function will return current cpu id
 */
int rt_hw_cpu_id(void)
{
    return TRICORE_CPU_ID;
}

__attribute__((noinline)) static void tricore_systick_handler( void )
{
    static int count = 0;  // ✅ 移到函数开头

    /* enter interrupt */
    rt_interrupt_enter();

    IfxStm_Timer_acknowledgeTimerIrq(&tricore_timers[TRICORE_CPU_ID]);

    rt_tick_increase();

    count++;  // ✅ 计数器递增

    /* leave interrupt */
    rt_interrupt_leave();
}


IFX_INTERRUPT(KERNEL_INTERRUPT, 0, 2)
{
    tricore_systick_handler();
}

IFX_INTERRUPT(KERNEL_YIELD, 0, 1)
{
    trigger_scheduling();
}

/**
 * This function will initial STM32 board.
 */
#ifdef RT_USING_HEAP
#ifdef __TASKING__
/* TASKING compiler uses different heap symbols */
extern unsigned char _lc_ub_heap[];
extern unsigned char _lc_ue_heap[];
#define __HEAP ((unsigned int*)_lc_ub_heap)
#define __HEAP_END ((unsigned int*)_lc_ue_heap)
#else
/* GCC compiler heap symbols */
extern unsigned int __HEAP[];
extern unsigned int __HEAP_END[];
#endif
#endif
void rt_hw_board_init()
{
    IfxStm_setSuspendMode(STMs[TRICORE_CPU_ID], IfxStm_SuspendMode_hard);
#ifdef RT_USING_HEAP
    /* initialize heap */
    rt_system_heap_init(__HEAP, __HEAP_END);
#endif

    /* Set-up the timer interrupt. */
    rt_hw_systick_init();
    /* USART driver initialization is open by default */
#ifdef RT_USING_SERIAL
    // rt_hw_usart_init();
#endif

    /* Set the shell console output device */
#ifdef RT_USING_CONSOLE
    // rt_console_set_device(RT_CONSOLE_DEVICE_NAME);
#endif

    /* Board underlying hardware initialization */
#ifdef RT_USING_COMPONENTS_INIT
    rt_components_board_init();
#endif

    IfxSrc_init(GPSR[TRICORE_CPU_ID], (IfxSrc_Tos)TRICORE_CPU_ID, 1);
    IfxSrc_enable(GPSR[TRICORE_CPU_ID]);
}

void rt_hw_context_switch(rt_ubase_t from, rt_ubase_t to)
{
    rt_kprintf("[DEBUG] rt_hw_context_switch called, from=0x%x, to=0x%x\n", from, to);
    
    if (rt_thread_switch_interrupt_flag == 0)
        rt_thread_switch_interrupt_flag = 1;

    if (rt_thread_switch_interrupt_flag == 1)
    {
        rt_interrupt_from_thread = from;
    }

    rt_interrupt_to_thread = to;

    /* 设置上下文切换标志，让调度器处理实际的切换 */
    rt_kprintf("[DEBUG] Context switch parameters set\n");
}

void rt_hw_context_switch_interrupt(rt_ubase_t from, rt_ubase_t to)
{
    if(rt_thread_switch_interrupt_flag == 0)
    {
        /* 将from和to线程保存到对应的全局变量中，以便于后续线程切换 */
        rt_interrupt_from_thread = (*( (unsigned long *)from ));
        rt_interrupt_to_thread = (*((unsigned long *)to));
        /* 置位中断切换线程Flag */
        rt_thread_switch_interrupt_flag = 1;
        __dsync();
        /* SETR置位，触发中断，在中断函数中请求线程切换 */
        GPSR[TRICORE_CPU_ID]->B.SETR = 1;
        __isync();
    }
}

void rt_hw_context_switch_to(rt_ubase_t to)
{
    rt_interrupt_to_thread = to;
    rt_interrupt_from_thread = 0;
    
    rt_kprintf("[DEBUG] rt_hw_context_switch_to called, to=0x%x\n", to);
    
    /* 启动第一个线程 */
    rt_kprintf("[DEBUG] Starting first thread\n");
    
    /* 禁用中断 */
    rt_base_t level = rt_hw_interrupt_disable();
    
    /* 获取目标线程的栈指针 */
    rt_uint8_t **thread_sp = (rt_uint8_t **)to;
    rt_uint8_t *sp = *thread_sp;
    
    rt_kprintf("[DEBUG] Thread stack pointer: 0x%x\n", (rt_ubase_t)sp);
    
    /* 获取栈顶的CSA链接字 */
    unsigned long csa_linkword = *((unsigned long *)sp);
    unsigned long *lower_csa = LINKWORD_TO_ADDRESS(csa_linkword);
    
    if (lower_csa != NULL) {
        /* 从CSA中获取线程入口点和参数 */
        void (*thread_entry)(void *) = (void (*)(void *))lower_csa[1];  /* PC指针 */
        void *parameter = (void *)lower_csa[8];  /* 参数 */
        
        rt_kprintf("[DEBUG] Thread entry: 0x%x, parameter: 0x%x\n", 
                   (rt_ubase_t)thread_entry, (rt_ubase_t)parameter);
        
        /* 恢复中断状态 */
        rt_hw_interrupt_enable(level);
        
        /* 直接调用线程入口函数 */
        if (thread_entry != NULL) {
            thread_entry(parameter);
        }
    } else {
        rt_kprintf("[ERROR] Invalid CSA linkword: 0x%x\n", csa_linkword);
        rt_hw_interrupt_enable(level);
    }
}

/**
 * This function will initialize thread stack
 *
 * @param tentry the entry of thread
 * @param parameter the parameter of entry
 * @param stack_addr the beginning stack address
 * @param texit the function will be called when thread exit
 *
 * @return stack address
 */
rt_uint8_t *rt_hw_stack_init(void       *tentry,
                             void       *parameter,
                             rt_uint8_t *stack_addr,
                             void       *texit)
{
    unsigned long *lower_csa = NULL;
    unsigned long *upper_csa = NULL;

    rt_hw_interrupt_disable();
    {
        __dsync();
        /* 使用两个空闲CSA存储线程上层上下文和下层上下文 */
        upper_csa = LINKWORD_TO_ADDRESS(__mfcr(CPU_FCX));

        if( NULL != upper_csa )
        {
            /* 查找上层上下文链接的下一个CSA地址 */
            lower_csa = LINKWORD_TO_ADDRESS( upper_csa[ 0 ] );
        }

        /* 如果成功分配两个CSA，则移动FCX */
        if( ( NULL != lower_csa ) && ( NULL != upper_csa ))
        {
            /* 将已经使用的两个CSA，从FCX列表中移除 */
            __disable();
            __dsync();
            __mtcr( CPU_FCX, lower_csa[ 0 ] );

            __isync();
            __enable();
        }
        else
        {
            __svlcx();
        }
    }
    rt_hw_interrupt_enable((rt_base_t)RT_NULL);

    /* 保存线程退出函数入口 */
    upper_csa[ 3 ] = ( unsigned long )texit;
    /* 保存栈指针，对应A[10]寄存器 */
    upper_csa[ 2 ] = ( unsigned long )stack_addr;
    /* 保存当前线程PSW寄存器的初始状态 */
    upper_csa[ 1 ] = TRICORE_SYSTEM_PROGRAM_STATUS_WORD;

    /* 保存函数入参，对应全局寄存器A[4] */
    lower_csa[ 8 ] = ( unsigned long ) parameter;
    /* 保存 PC 指针，A[11]*/
    lower_csa[ 1 ] = ( unsigned long ) tentry;
    /* PCXI指向上层上下文 */
    lower_csa[ 0 ] = ( TRICORE_INITIAL_PCXI_UPPER_CONTEXT_WORD | ( unsigned long ) ADDRESS_TO_LINKWORD( upper_csa ) );
    /* 将下层上下文的地址保存到栈顶 */
    *((unsigned long * )stack_addr) = (unsigned long) ADDRESS_TO_LINKWORD( lower_csa );
    __dsync();

    return (rt_uint8_t*)stack_addr;
}


//void get_clk(void)
//{
//	disableInterrupts();
//	//绂佺敤鐪嬮棬鐙�
//    IfxScuWdt_disableCpuWatchdog(IfxScuWdt_getCpuWatchdogPassword());
//    IfxScuWdt_disableSafetyWatchdog(IfxScuWdt_getSafetyWatchdogPassword());
//
//    //鑾峰彇鏃堕挓棰戠巼锛屼究浜庢煡鐪嬪綋鍓嶇郴缁熻繍琛岄鐜�
//    g_AppCpu0.info.pllFreq = IfxScuCcu_getPllFrequency();
//    g_AppCpu0.info.cpuFreq = IfxScuCcu_getCpuFrequency(IfxCpu_getCoreIndex());
//    g_AppCpu0.info.sysFreq = IfxScuCcu_getSpbFrequency();
//    g_AppCpu0.info.stmFreq = IfxStm_getFrequency(&MODULE_STM0);
//}


